function q = IKine(P0, Pe)
q1 = atan2(Pe(1) - P0(1), P0(3) - Pe(3));
L = norm(P0 - Pe);
d1 = 1; 
q2 = L - d1;
q = [q1; q2];